#include "can.h"
#include "led.h"
#include "delay.h"
#include "usart.h"
#include "encoder.h"
#include "math.h"
#include "usart3.h"
#include "usart3.h"
#include "spi.h"
#include "OpenImu.h"
#include "timer.h"
#include "fault_code.h"
/*****************************新增***********************/

u8 Send_OK = 1; //1表示发送失败，0表示发送成功
CanRxMsg_new CanRxMsg_new_store;
u8 Send_Retutn_data[R_Len];
u8 Angle[16];
u8 Zero_Status[Zero_S_Len];
Encoder Encoder_All;
Frame_Sign Frame_Sign_usart3;
u8 usart3_Receive_Data[Usart3_R_Len];

Encoder Encoder_1 = {0.0, 0.0, Encoder_ID_1, 1, "0", 1};
Encoder Encoder_2 = {0.0, 0.0, Encoder_ID_2, 1, "0", 1};
Encoder Encoder_3 = {0.0, 0.0, Encoder_ID_3, 1, "0", 1};
Encoder Encoder_4 = {0.0, 0.0, Encoder_ID_4, 1, "0", 1};

int pow_int(int m, int n)
{
    int r = 1, i;
    for (i = 0; i < n; i++) //循环执行n次。
        r *= m;             //每次乘一次m，结果即m的n次方。
    return r;               //返回结果。
}

void EncoderInit(void)
{
   Frame_Sign_usart3.now_status = Usart_Initial_number;                      //设置串口帧初始化状态为空
}

//发送读取几号编码器角度命令(固定格式:参数can_id为几号编码器,标准帧,数据帧)
//len:数据长度(最大为8)
//返回值:0,成功;
//     其他,失败;
u8 CAN1_Send_Read_Angle_Order(u8 can_id)
{
    u8 mbox;
    u16 i = 0;
    u8 len = 4;

    CanTxMsg TxMessage;
    TxMessage.StdId = can_id; //0x12;   // 标准标识符为0
    TxMessage.ExtId = can_id; //0x12;   // 设置扩展标示符（29位）
    TxMessage.IDE = 0;        // 使用扩展标识符
    TxMessage.RTR = 0;        // 消息类型为数据帧，一帧8位
    TxMessage.DLC = len;      // 发送两帧信息
    TxMessage.Data[0] = len;
    TxMessage.Data[1] = can_id;
    TxMessage.Data[2] = 0x01;
    TxMessage.Data[3] = 0x00;
    for (i = 4; i < len; i++)
        TxMessage.Data[i] = 0x00; // 第一帧信息
    mbox = CAN_Transmit(CAN1, &TxMessage);
    i = 0;
    while ((CAN_TransmitStatus(CAN1, mbox) == CAN_TxStatus_Failed) && (i < 0XFFF))
        i++; //等待发送结束

    if (i >= 0XFFF)
    {
        Send_OK = 1;
        return 1;
    }
    else
    {
        Send_OK = 0;
        return 0;
    }
}

//发送设置几号编码器零点命令(固定格式:参数can_id为几号编码器,标准帧,数据帧)
//len:数据长度(最大为8)
//返回值:0,成功;
//     其他,失败;
u8 CAN1_Send_Set_Zero_Order(u8 can_id)
{
    u8 mbox;
    u16 i = 0;
    u8 len = 4;

    CanTxMsg TxMessage;
    TxMessage.StdId = can_id; //0x12;   // 标准标识符为0
    TxMessage.ExtId = can_id; //0x12;   // 设置扩展标示符（29位）
    TxMessage.IDE = 0;        // 使用扩展标识符
    TxMessage.RTR = 0;        // 消息类型为数据帧，一帧8位
    TxMessage.DLC = len;      // 发送两帧信息
    TxMessage.Data[0] = len;
    TxMessage.Data[1] = can_id;
    TxMessage.Data[2] = 0x06;
    TxMessage.Data[3] = 0x00;
    for (i = 4; i < len; i++)
        TxMessage.Data[i] = 0x00; // 第一帧信息
    mbox = CAN_Transmit(CAN1, &TxMessage);
    i = 0;
    while ((CAN_TransmitStatus(CAN1, mbox) == CAN_TxStatus_Failed) && (i < 0XFFF))
        i++; //等待发送结束
    if (i >= 0XFFF)
    {
        Send_OK = 1;
        return 1;
    }
    Send_OK = 0;
    return 0;
}

//发送设置几号编码器ID命令(固定格式:参数can_id为几号编码器,标准帧,数据帧)
//len:数据长度(最大为8)
//返回值:0,成功;
//     其他,失败;
u8 CAN1_Send_Set_ID_Order(u8 can_old_Id, u8 can_new_Id)
{
    u8 mbox;
    u16 i = 0;
    u8 len = 4;

    CanTxMsg TxMessage;
    TxMessage.StdId = can_old_Id; //0x12;   // 标准标识符为0
    TxMessage.ExtId = can_old_Id; //0x12;   // 设置扩展标示符（29位）
    TxMessage.IDE = 0;            // 使用扩展标识符
    TxMessage.RTR = 0;            // 消息类型为数据帧，一帧8位
    TxMessage.DLC = len;          // 发送两帧信息
    TxMessage.Data[0] = len;
    TxMessage.Data[1] = can_old_Id;
    TxMessage.Data[2] = 0x02;
    TxMessage.Data[3] = can_new_Id;
    for (i = 4; i < len; i++)
        TxMessage.Data[i] = 0x00; // 第一帧信息
    mbox = CAN_Transmit(CAN1, &TxMessage);
    i = 0;
    while ((CAN_TransmitStatus(CAN1, mbox) == CAN_TxStatus_Failed) && (i < 0XFFF))
        i++; //等待发送结束
    if (i >= 0XFFF)
    {
        Send_OK = 1;
        return 1;
    }
    Send_OK = 0;
    return 0;
}

//编码器标定程序 ，出厂时使用
u8 Encoder_Demarcate_Zero()
{
    CAN1_Send_Set_Zero_Order(Encoder_ID_1);
    while (Encoder_1.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_1);
        break;
    };
    CAN1_Send_Set_Zero_Order(Encoder_ID_2);
    while (Encoder_2.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_2);
        break;
    };
    CAN1_Send_Set_Zero_Order(Encoder_ID_3);
    while (Encoder_3.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_3);
        break;
    };
    CAN1_Send_Set_Zero_Order(Encoder_ID_4);
    while (Encoder_4.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_4);
        break;
    };
    Encoder_1.Encoder_CAN_Read_OK = 1;
    Encoder_2.Encoder_CAN_Read_OK = 1;
    Encoder_3.Encoder_CAN_Read_OK = 1;
    Encoder_4.Encoder_CAN_Read_OK = 1;
    return 0;
}

/**
  * @brief  标定指定编码器
  * @param  id：  要标定的编码器ID，目前支持2-5；其余不支持
  * @retval 解析的结果 1：失败，0：成功
  */
u8 Encoder_Demarcate(u8 id)
{
    Encoder *encoder = NULL;
    switch(id)
    {
        case 0x02 : encoder = &Encoder_1;
        case 0x03 : encoder = &Encoder_2;
        case 0x04 : encoder = &Encoder_3;
        case 0x05 : encoder = &Encoder_4;
    }
    if(id>=0x02 && id<=0x05)
    {
        CAN1_Send_Set_Zero_Order(id);
        while (encoder->Encoder_CAN_Read_OK)
        {
            Encoder_delay_1ms(1, encoder);
            break;
        };
        if(encoder->Encoder_CAN_Read_OK)
        {
          return 1;
        }
        else
        {
          encoder->Encoder_CAN_Read_OK = 1;
          return 0;
        }
    }
    return 1;
}



//F1串口3发送编码器角度于F4
//len:数据长度(最大为8)
//msg:数据指针,最大为8个字节.
//返回值:0,成功;
//     其他,失败;
u8 Encoder_Uart3_Return_Angle_Data(void)
{
    u8 sum = 0;
    u8 i = 0;
    u8 n = 0;
    int AngleData = 0; //IMU角度数据
    float *fAngle;

    Send_Retutn_data[0] = 0x0d;
    Send_Retutn_data[1] = 0x0a;
    Send_Retutn_data[2] = 0x01;
    /***************填充编码器角度数据到返回F4数组*********/
    for (i = 0; i < 4; i++)
    {
        for (n = 0; n < 4; n++)
            Send_Retutn_data[3 + i * 4 + n] = Angle[n + i * 4];
    }

    /***************填充2号IMU数据将float型原始数据*1000转换为整型，拆分为4个字节放入返回数组*********/
    fAngle = (float *)&Send_Retutn_data[19];
    *fAngle = OpenImu::get()->OpenImu_Data.Z_Axis.Angle;

    AngleData = int(OpenImu::get()->OpenImu_Data.Z_Axis.AngelVel * 1000);
    for (i = 0; i < 4; i++)
    {
        Send_Retutn_data[23 + i] = (AngleData >> (i * 8)) & 0x000000FF; //转换角速度
    }
    /***************************************************************************************************/

    /*****************填充F1 4个编码器，IMU，串口通信故障状态********************/
    for (i = 0; i < 4; i++)
    {
        Send_Retutn_data[26 + i] = WingbotFault.EncoderWingbot.EncoderSensor[i].err_status;
    }
    Send_Retutn_data[30] = WingbotFault.ImuWingbot.Imu[0].err_status;
    Send_Retutn_data[31] = WingbotFault.UartWingbot.Uart[0].err_status;
    /****************************************************************************/

    for (i = 0; i < Encoder_Imu_Len - 5; i++)
        sum += Send_Retutn_data[2 + i];
    Send_Retutn_data[Encoder_Imu_Len - 3] = sum;
    Send_Retutn_data[Encoder_Imu_Len - 2] = 0x5a;
    Send_Retutn_data[Encoder_Imu_Len - 1] = 0xa5;

    UART3_Send_Array(Send_Retutn_data, Encoder_Imu_Len);

    Frame_Sign_usart3.now_status = Usart_Initial_number;

    return 0;
}

//F1串口3发送给F4编码器零点是否设置成功
//len:数据长度(最大为8)
//msg:数据指针,最大为8个字节.
//返回值:1,成功;
//     0,失败;
u8 Encoder_Uart3_Return_Zero_Status(void)
{
    u8 sum = 0;
    u8 i = 0;

    Send_Retutn_data[0] = 0x0d;
    Send_Retutn_data[1] = 0x0a;
    Send_Retutn_data[2] = 0x02;
    for (i = 0; i < 5; i++)
    {
        Send_Retutn_data[3 + i] = Zero_Status[i];
    }

    for (i = 0; i < Encoder_Imu_Len - 5; i++)
        sum += Send_Retutn_data[2 + i];
    Send_Retutn_data[Encoder_Imu_Len - 3] = sum;
    Send_Retutn_data[Encoder_Imu_Len - 2] = 0x5a;
    Send_Retutn_data[Encoder_Imu_Len - 1] = 0xa5;

    UART3_Send_Array(Send_Retutn_data, Encoder_Imu_Len);
    for (i = 0; i < Zero_S_Len; i++)
    {
        Zero_Status[i] = 0x00;
    }
    Frame_Sign_usart3.now_status = Usart_Initial_number; //代表一帧发送结束

    return 0;
}

//计算编码器角度
//len:数据长度(最大为8)
//msg:数据指针,最大为8个字节.
//返回值:0,成功;
//     其他,失败;
u8 Caculate_Encoder_Angle_Data(u8 can_id)
{
    float sum = 0;
    float cycle = 0;
    float angle = 0;
    u8 i = 0;
    int c1 = 0;
    // printf("命令:%x\r\n",CanRxMsg_new_store.Data[2]);
    if (CanRxMsg_new_store.Data[2] == Encoder_Angle_Order)
    {
        for (i = 0; i < 4; i++)
        {
            sum += (CanRxMsg_new_store.Data[3 + i] * pow_int(256, i));
        }
        //printf("圈数:%d\r\n",sum);
    }
    cycle = sum / 4096;
    angle = cycle * 360;
    Encoder_All.cycle = sum / 4096;
    Encoder_All.angle = Encoder_All.cycle * 360;

    switch (can_id)
    {
    case Encoder_ID_1:
        Encoder_1.Encoder_CAN_Read_OK = 0;
        Encoder_1.angle = angle;
        break;
    case Encoder_ID_2:
        Encoder_2.Encoder_CAN_Read_OK = 0;
        Encoder_2.angle = angle;
        break;
    case Encoder_ID_3:
        Encoder_3.Encoder_CAN_Read_OK = 0;
        Encoder_3.angle = angle;
        break;
    case Encoder_ID_4:
        Encoder_4.Encoder_CAN_Read_OK = 0;
        Encoder_4.angle = angle;
        break;
    default:
        break; // CanRxMsg_new_store.Data[3]=0x02;break;
    }

    if ((angle <= ENCODER_MAX_ANGLE) && (angle >= ENCODER_HALF_MAX_ANGLE)) //如果大于4320到8640之间，则认为小车编码器在反转，因为小车最多只能转1圈360度
    {
        angle = (angle - ENCODER_MAX_ANGLE) * ENCODER_SMALL_GEAR / ENCODER_BIG_GEAR; //计算相对角度; 认为是反转为负
    }
    else
        angle = (angle)*ENCODER_SMALL_GEAR / ENCODER_BIG_GEAR; //计算相对角度    //认为正转为正

    //   angle = NormalizeAngle(angle) * ENCODER_SMALL_GEAR / ENCODER_BIG_GEAR;    //计算相对角度

    c1 = (int)((angle + EncoderRelativeAngle) * 100); //角度*100回传到F4
    Caculate_Update(can_id, c1);                      //更新倍数角度到云数组
                                                      // for(i=0;i<16;i++)

    // printf("Angle[%d]:%d\r\n",i,Angle[i]);
    // printf("圈数:%f\r\n",Encoder_All.cycle);
    // printf("角度:%f\r\n",Encoder_All.angle);

    return 0;
}

/******************************************************************************************/
//判断CRC是否成功
////返回：0表示成功
//        1表示失败
u8 Judge_CRC()
{
    u8 i;
    u32 sum = 0;
    for (i = 0; i < 7; i++)
    {
        sum += usart3_Receive_Data[2 + i];
    }
    //printf("Crc_Sum:%d\r\n",sum);
    if (sum == usart3_Receive_Data[9])
    {
        //    printf("CRC成功\r\n");
        sum = 0;
        return 0;
    }
    else
        //    printf("CRC失败\r\n");
        sum = 0;
    return 1;
}

/*******************************解析F4发送的指令**************************************************/

void Analysis_Order()
{
    USART_Receive_Queue_Extract(USART3_RX_BUF); //从队列中提取一帧数据  ，并更新一次 usart3_Receive_Data[]处理数组

    if (Procesee_Receive_Queue != 0) //如果待处理进程不为0
    {
        switch (usart3_Receive_Data[2]) //处理对应指令
        {
        case F4_Angle_Order:
            OpenImu::get()->getWingbotVel(usart3_Receive_Data); //获得小车状态，是否静止
            Encoder_Uart3_Return_Angle_Data();                  //返回编码器角度和IMU角度

            break;
        case F4_Zero_Order:
            Analyze_Zero(usart3_Receive_Data);

            Encoder_Uart3_Return_Zero_Status();

            break;
        case F4_ID_Order:

            break;

        case F4_Angle_Imu_Order:
            OpenImu::get()->OpenImu_Analysis_F4_Order(usart3_Receive_Data);
            break;
        case F4_Zero_Imu_Order:
            OpenImu::get()->OpenImu_Analysis_F4_Order(usart3_Receive_Data);
            break;
        case F4_OffestAngle_Imu_Order:
            OpenImu::get()->OpenImu_Analysis_F4_Order(usart3_Receive_Data);
            break;

        default:
            Frame_Sign_usart3.now_status = Usart_Initial_number;
            break;
        }

        for (u8 i = 0; i < 12; i++)
        {
            usart3_Receive_Data[i] = 0x00; //处理完一条指令，进程数组清零
        }
    }
    else
        ;
}

/***********************************轮询查询角度**********************************************/

u8 loop_qurey_angle()
{
    CAN1_Send_Read_Angle_Order(Encoder_ID_1);
    while (Encoder_1.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_1);
        break;
    };
    CAN1_Send_Read_Angle_Order(Encoder_ID_2);
    while (Encoder_2.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_2);
        break;
    };
    CAN1_Send_Read_Angle_Order(Encoder_ID_3);
    while (Encoder_3.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_3);
        break;
    };
    CAN1_Send_Read_Angle_Order(Encoder_ID_4);
    while (Encoder_4.Encoder_CAN_Read_OK)
    {
        Encoder_delay_1ms(1, &Encoder_4);
        break;
    };
    Encoder_1.Encoder_CAN_Read_OK = 1;
    Encoder_2.Encoder_CAN_Read_OK = 1;
    Encoder_3.Encoder_CAN_Read_OK = 1;
    Encoder_4.Encoder_CAN_Read_OK = 1;

    return 0;
}

/*******************更新角度到云----角度数组****************************/

void Caculate_Update(u8 can_id, int I_Angle)
{
    //  I_Angle = NormalizeAngle(I_Angle) * ENCODER_SMALL_GEAR / ENCODER_BIG_GEAR;

    Angle[3 + (can_id - 2) * 4] = (I_Angle >> 24) & 0xff;
    Angle[2 + (can_id - 2) * 4] = (I_Angle >> 16) & 0xff;
    Angle[1 + (can_id - 2) * 4] = (I_Angle >> 8) & 0xff;
    Angle[0 + (can_id - 2) * 4] = (I_Angle) & (0xff);
}

/***********************解析F4发送来的零点设置命令*****************/

void Analyze_Zero(u8 Zero_Order[])
{
    u8 i = 0;
    //清零角度编码器
    for (i = 0; i < 4; i++)
    {
        switch (Zero_Order[i + 3])
        {
        case 0x01:
            CAN1_Send_Set_Zero_Order(i + 2);
            Judge_Which_Enocder(i);
            break;
        case 0x00:
            break;
        default:
            break;
        }
    }

    //清零IMU
    for (i = 0; i < 2; i++)
    {
        switch (Zero_Order[i + 7])
        {
        case 0x01:
            OpenImu::get()->ImuData_Clear();
            break;
        case 0x00:
            break;
        default:
            break;
        }
    }
}

/*********************判断零点命令下发送CAN命令是哪个编码器等待返回成功***************************/

void Judge_Which_Enocder(u8 nn)
{
    switch (nn)
    {
    case 0:
        while (Encoder_1.Encoder_CAN_Read_OK)
        {
            Encoder_delay_1ms(1, &Encoder_1);
            break;
        }
        Encoder_1.Encoder_CAN_Read_OK = 1;
        break;
    case 1:
        while (Encoder_2.Encoder_CAN_Read_OK)
        {
            Encoder_delay_1ms(1, &Encoder_2);
            break;
        }
        Encoder_2.Encoder_CAN_Read_OK = 1;
        break;
    case 2:
        while (Encoder_3.Encoder_CAN_Read_OK)
        {
            Encoder_delay_1ms(1, &Encoder_3);
            break;
        }
        Encoder_3.Encoder_CAN_Read_OK = 1;
        break;
    case 3:
        while (Encoder_4.Encoder_CAN_Read_OK)
        {
            Encoder_delay_1ms(1, &Encoder_4);
            break;
        }
        Encoder_4.Encoder_CAN_Read_OK = 1;
        break;
    default:
        break;
    }
}

//CAN中断返回值判断编码器零点设置返回值是否成功并更新零点是否设置成功到-----编码器零点记录云数组
//返回值:0,成功;
//     其他,失败;
u8 Caculate_Encoder_Zero_Data(u8 can_id)
{
    if ((CanRxMsg_new_store.Data[2] == Encoder_Zero_Order) && (CanRxMsg_new_store.Data[3] == 0x00))
    {
        switch (can_id)
        {
        case Encoder_ID_1:
            Encoder_1.Encoder_CAN_Read_OK = 0;
            Encoder_1.ZERO = 0x01;
            break;
        case Encoder_ID_2:
            Encoder_2.Encoder_CAN_Read_OK = 0;
            Encoder_2.ZERO = 0x01;
            break;
        case Encoder_ID_3:
            Encoder_3.Encoder_CAN_Read_OK = 0;
            Encoder_3.ZERO = 0x01;
            break;
        case Encoder_ID_4:
            Encoder_4.Encoder_CAN_Read_OK = 0;
            Encoder_4.ZERO = 0x01;
            break;
        default:
            break; // CanRxMsg_new_store.Data[3]=0x02;break;
        }
        Zero_Status[can_id - 2] = 0x01;
    }
    else
    {
        Zero_Status[can_id - 2] = 0x00;
    }
    return 0;
}

/**
  * @brief  编码器主循环判断是否返回数据的延时，小于1ms时间或大于1ms内会自动置零Encoder_CAN_Read_OK，防止卡死在while循环
  * @param  time:  ms，要循环的次数；Encoder_Read_Flag： 要读取的相应的编码器标识
  * @retval 1失败，0成功
  */
u8 Encoder_delay_1ms(u8 time, Encoder *encoder)
{
    u16 i = 0;
    i = 10000; //自己定义
    u16 n = 0;
    while (time--) //等待超时时间到达
    {
        if (encoder->Encoder_CAN_Read_OK)
        {
            while (i--)
            {
                n++;
                if (encoder->Encoder_CAN_Read_OK == 0)
                {
                    n = 0x8888;
                    break;
                }
            }
        }
        else
        {

            break;
        }
    }

    CheckEncoderFault(encoder); //监测编码器故障
    return encoder->Encoder_CAN_Read_OK;
}
